/***************************************************************************//**
 * @file MSc_Quaternion.cpp
 * @author  Marek M. Cel <marekcel@mscsim.org>
 *
 * @section LICENSE
 *
 * Copyright (C) 2013 Marek M. Cel
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 ******************************************************************************/
#ifndef MSC_QUATERNION_CPP
#define MSC_QUATERNION_CPP
#endif

////////////////////////////////////////////////////////////////////////////////

#ifndef _USE_MATH_DEFINES
#   define _USE_MATH_DEFINES
#endif

////////////////////////////////////////////////////////////////////////////////

#include <cmath>
#include <iostream>
#include <sstream>

#include <utils/MSc_Matrix3x3.h>
#include <utils/MSc_Quaternion.h>
#include <utils/MSc_Utils.h>

////////////////////////////////////////////////////////////////////////////////

MSc_Quaternion::MSc_Quaternion()
{
    m_w = 0.0;
    m_x = 0.0;
    m_y = 0.0;
    m_z = 0.0;
}

////////////////////////////////////////////////////////////////////////////////

MSc_Quaternion::MSc_Quaternion( const MSc_Quaternion &qtrn )
{
    set( qtrn.m_w, qtrn.m_x, qtrn.m_y, qtrn.m_z );
}

////////////////////////////////////////////////////////////////////////////////

MSc_Quaternion::MSc_Quaternion( const MSc_Quaternion *qtrn )
{
    set( qtrn->m_w, qtrn->m_x, qtrn->m_y, qtrn->m_z );
}

////////////////////////////////////////////////////////////////////////////////

MSc_Quaternion MSc_Quaternion::getFrom( double w, double x, double y, double z )
{
    MSc_Quaternion result;

    result.set( w, x, y, z );

    return result;
}

////////////////////////////////////////////////////////////////////////////////

MSc_Quaternion MSc_Quaternion::getFrom( double phi, double tht, double psi )
{
    MSc_Quaternion result;

    result.setAngles( phi, tht, psi );

    return result;
}

////////////////////////////////////////////////////////////////////////////////

MSc_Quaternion MSc_Quaternion::getFrom( const MSc_Angles &angl )
{
    MSc_Quaternion result;

    result.setAngles( angl.phi(), angl.tht(), angl.psi() );

    return result;
}

////////////////////////////////////////////////////////////////////////////////

bool MSc_Quaternion::isValid() const
{
    return MSc_Utils::isValid( m_w )
        && MSc_Utils::isValid( m_x )
        && MSc_Utils::isValid( m_y )
        && MSc_Utils::isValid( m_z );
}

////////////////////////////////////////////////////////////////////////////////

MSc_Angles MSc_Quaternion::getAngles() const
{
    double phiTemp;
    double thtTemp;
    double psiTemp;

    getAngles( &phiTemp, &thtTemp, &psiTemp );

    return MSc_Angles::getFrom( phiTemp, thtTemp, psiTemp );
}

////////////////////////////////////////////////////////////////////////////////

void MSc_Quaternion::getAngles( MSc_Angles &angl ) const
{
    angl = getAngles();
}

////////////////////////////////////////////////////////////////////////////////

void MSc_Quaternion::getAngles( MSc_Angles *angl ) const
{
    if ( angl ) { (*angl) = getAngles(); }
}

////////////////////////////////////////////////////////////////////////////////

void MSc_Quaternion::getAngles( double &phi, double &tht, double &psi ) const
{
    getAngles( &phi, &tht, &psi );
}

////////////////////////////////////////////////////////////////////////////////

void MSc_Quaternion::getAngles( double *phi, double *tht, double *psi ) const
{
    double phiTemp = 0.0;
    double thtTemp = 0.0;
    double psiTemp = 0.0;

    MSc_Matrix3x3 rotMat = MSc_Matrix3x3::getRotationMatrix( *this );

    if ( rotMat(2,2) == 0.0 )
    {
        phiTemp = 0.5 * M_PI;
    }
    else
    {
        phiTemp = atan2( rotMat(1,2), rotMat(2,2) );
    }

    if ( rotMat(0,2) < -1.0 )
    {
        thtTemp = 0.5 * M_PI;
    }
    else
    {
        if ( rotMat(0,2) > 1.0 )
        {
            thtTemp = -0.5 * M_PI;
        }
        else
        {
            thtTemp = asin( -rotMat(0,2) );
        }
    }

    if ( rotMat(0,0) == 0.0 )
    {
        psiTemp = 0.5 * M_PI;
    }
    else
    {
        psiTemp = atan2( rotMat( 0,1 ), rotMat(0,0) );

        if ( psiTemp < 0.0 ) psiTemp += 2.0 * M_PI;
    }

    (*phi) = phiTemp;
    (*tht) = thtTemp;
    (*psi) = psiTemp;
}

////////////////////////////////////////////////////////////////////////////////

void MSc_Quaternion::setAngles( double phi, double tht, double psi )
{
    double phi2 = phi / 2.0;
    double tht2 = tht / 2.0;
    double psi2 = psi / 2.0;

    double sinPhi2 = sin( phi2 );
    double cosPhi2 = cos( phi2 );

    double sinTht2 = sin( tht2 );
    double cosTht2 = cos( tht2 );

    double sinPsi2 = sin( psi2 );
    double cosPsi2 = cos( psi2 );

    double cosPhi2CosPsi2 = cosPhi2 * cosPsi2;
    double cosPhi2SinPsi2 = cosPhi2 * sinPsi2;
    double sinPhi2SinPsi2 = sinPhi2 * sinPsi2;
    double sinPhi2CosPsi2 = sinPhi2 * cosPsi2;

    m_w = ( cosPhi2CosPsi2 * cosTht2 ) + ( sinPhi2SinPsi2 * sinTht2 );
    m_x = ( sinPhi2CosPsi2 * cosTht2 ) - ( cosPhi2SinPsi2 * sinTht2 );
    m_y = ( cosPhi2CosPsi2 * sinTht2 ) + ( sinPhi2SinPsi2 * cosTht2 );
    m_z = ( cosPhi2SinPsi2 * cosTht2 ) - ( sinPhi2CosPsi2 * sinTht2 );
}

////////////////////////////////////////////////////////////////////////////////

void MSc_Quaternion::setAngles( const MSc_Angles &angl )
{
    setAngles( angl.phi(), angl.tht(), angl.psi() );
}

////////////////////////////////////////////////////////////////////////////////

void MSc_Quaternion::set( double w, double x, double y, double z )
{
    m_w = w;
    m_x = x;
    m_y = y;
    m_z = z;
}

////////////////////////////////////////////////////////////////////////////////

MSc_Quaternion& MSc_Quaternion::operator= ( const MSc_Quaternion &qtrn )
{
    m_w = qtrn.m_w;
    m_x = qtrn.m_x;
    m_y = qtrn.m_y;
    m_z = qtrn.m_z;

    return (*this);
}

////////////////////////////////////////////////////////////////////////////////

MSc_Quaternion MSc_Quaternion::operator+ ( const MSc_Quaternion &qtrn ) const
{
    MSc_Quaternion result;

    result.m_w = m_w + qtrn.m_w;
    result.m_x = m_x + qtrn.m_x;
    result.m_y = m_y + qtrn.m_y;
    result.m_z = m_z + qtrn.m_z;

    return result;
}

////////////////////////////////////////////////////////////////////////////////

MSc_Quaternion MSc_Quaternion::operator- () const
{
    MSc_Quaternion result;

    result.m_w = -m_w;
    result.m_x = -m_x;
    result.m_y = -m_y;
    result.m_z = -m_z;

    return result;
}

////////////////////////////////////////////////////////////////////////////////

MSc_Quaternion MSc_Quaternion::operator- ( const MSc_Quaternion &qtrn ) const
{
    MSc_Quaternion result;

    result.m_w = m_w - qtrn.m_w;
    result.m_x = m_x - qtrn.m_x;
    result.m_y = m_y - qtrn.m_y;
    result.m_z = m_z - qtrn.m_z;

    return result;
}

////////////////////////////////////////////////////////////////////////////////

MSc_Quaternion MSc_Quaternion::operator* ( double val ) const
{
    MSc_Quaternion result;

    result.m_w = m_w * val;
    result.m_x = m_x * val;
    result.m_y = m_y * val;
    result.m_z = m_z * val;

    return result;
}

////////////////////////////////////////////////////////////////////////////////

MSc_Quaternion MSc_Quaternion::operator/ ( double val ) const
{
    MSc_Quaternion result;

    result.m_w = m_w / val;
    result.m_x = m_x / val;
    result.m_y = m_y / val;
    result.m_z = m_z / val;

    return result;
}

////////////////////////////////////////////////////////////////////////////////

MSc_Quaternion MSc_Quaternion::operator% ( const MSc_Quaternion &qtrn ) const
{
    MSc_Quaternion result;

    result.m_w = m_w * qtrn.m_w
               - m_x * qtrn.m_x
               - m_y * qtrn.m_y
               - m_z * qtrn.m_z;

    result.m_x = m_w * qtrn.m_x
               + m_x * qtrn.m_w
               + m_y * qtrn.m_z
               - m_z * qtrn.m_y;

    result.m_y = m_w * qtrn.m_y
               - m_x * qtrn.m_z
               + m_y * qtrn.m_w
               + m_z * qtrn.m_x;

    result.m_z = m_w * qtrn.m_z
               + m_x * qtrn.m_y
               - m_y * qtrn.m_x
               + m_z * qtrn.m_w;

    return result;
}

////////////////////////////////////////////////////////////////////////////////

MSc_Quaternion& MSc_Quaternion::operator+= ( const MSc_Quaternion &qtrn )
{
    m_w += qtrn.m_w;
    m_x += qtrn.m_x;
    m_y += qtrn.m_y;
    m_z += qtrn.m_z;

    return (*this);
}

////////////////////////////////////////////////////////////////////////////////

MSc_Quaternion& MSc_Quaternion::operator-= ( const MSc_Quaternion &qtrn )
{
    m_w -= qtrn.m_w;
    m_x -= qtrn.m_x;
    m_y -= qtrn.m_y;
    m_z -= qtrn.m_z;

    return (*this);
}

////////////////////////////////////////////////////////////////////////////////

MSc_Quaternion& MSc_Quaternion::operator*= ( double val )
{
    m_w *= val;
    m_x *= val;
    m_y *= val;
    m_z *= val;

    return (*this);
}

////////////////////////////////////////////////////////////////////////////////

MSc_Quaternion& MSc_Quaternion::operator/= ( double val )
{
    m_w /= val;
    m_x /= val;
    m_y /= val;
    m_z /= val;

    return (*this);
}

////////////////////////////////////////////////////////////////////////////////

bool MSc_Quaternion::operator== ( const MSc_Quaternion &qtrn ) const
{
    return ( m_w == qtrn.m_w ) && ( m_x == qtrn.m_x ) && ( m_y == qtrn.m_y ) && ( m_z == qtrn.m_z );
}

////////////////////////////////////////////////////////////////////////////////

bool MSc_Quaternion::operator!= ( const MSc_Quaternion &qtrn ) const
{
    return !( (*this) == qtrn );
}
